/**
 ******************************************************************************
 * @file    *.c
 * @author  pengli.wang
 * @version V3.5.0
 * @date    08-April-2018
 * @brief   Handle the radar output data .
 ******************************************************************************
 * @attention
 *
 * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
 * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
 * TIME. AS A RESULT, HURYS SHALL NOT BE HELD LIABLE FOR ANY
 * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
 * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
 * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
 *
 * <h2><center>&copy; COPYRIGHT 2018 HURYS</center></h2>
 ******************************************************************************
 */
#include "arsx08.h"

union CANTxMsgData {
    uint8_t               Data[8];
    struct MsgRadarConfig MsgRadarConfigs;
};

union CANRadarState {
    uint8_t              Data[8];
    struct MsgRadarState MsgRadarStates;
};

ClusterList_t     clusterBuff[20]        = {0};
MsgObjectStatus_t objStatus              = {0};
ObjStruct_t       objMovingData[10]      = {0};
ObjStruct_t       objOncomingData[10]    = {0};
ObjStruct_t       objOncomingBuff[10]    = {0};
ObjStruct_t       objCrossmovingData[10] = {0};
uint16_t          radarChangeFlag        = 0;
u8                objCouonts             = 0;
u8                movingCount            = 0;
u8                oncomingCount          = 0;
u8                crossmovingCount       = 0;

/*
Init ARS Radar Configration when the program load.
noticed that the sensor's ID must be 0.
*/

/*
Init ARS Radar Configration when the program load.
noticed that the sensor's ID must be 0.
*/
void ARS_Init(void)
{
    u8 DataBuf[8] = {0};
    DataBuf[0]    = 0xC8;
    DataBuf[1]    = 0x00;
    DataBuf[2]    = 0x00;
    DataBuf[3]    = 0x00;
    DataBuf[4]    = 0x08;
    DataBuf[5]    = 0x90;
    DataBuf[6]    = 0x00;
    DataBuf[7]    = 0x00;

    while (1) {
        if (Can_Send_Msg(0x0200, 0, DataBuf, 8, CAN_ID_STD, CAN_RTR_DATA)) {
            bsp_DelayMS(100);
            continue;
        }
        break;
    }
}

/*解析0x0201数据*/
struct MsgRadarState GetRadarState(CanRxMsg *canrxMsg)
{
    uint8_t             i = 0;
    union CANRadarState canradarstate;
    for (; i < 8; i++) {
        canradarstate.Data[i] = canrxMsg->Data[i];
    }
    return canradarstate.MsgRadarStates;
}
/**
 * @brief   0x60a
 * @param  CANRxMsg         My Param doc
 */
void ARSObjStatusHandle(CanRxMsg *CANRxMsg)
{
    objStatus.NoOfObjects = CANRxMsg->Data[0];
    objStatus.MeasCounter = CANRxMsg->Data[2];

    // printf("NoOfObjects:%02d \r\n", objStatus.NoOfObjects);

    movingCount      = 0;
    crossmovingCount = 0;
    oncomingCount    = 0;
    objCouonts       = 0;
}

/**
 * @brief Handle 0x60b
 *
 * @param CANRxMsg
 * @return struct ObjList
 */
struct ObjList ARS_Obj_Handle(CanRxMsg *CANRxMsg)
{
    ObjStruct_t    objData;
    struct ObjList CAN1_Obj_1_Msg;
    u16            cache = 0;
    double         x, y;

    CAN1_Obj_1_Msg.Obj_ID        = (CANRxMsg->Data[0] >> 0);
    CAN1_Obj_1_Msg.Obj_LongDispl = ((CANRxMsg->Data[1] << 5) | (CANRxMsg->Data[2] >> 3)) * 0.2 - 500;
    CAN1_Obj_1_Msg.Obj_LatDispl  = ((((CANRxMsg->Data[2] & 0x07) << 8) | (CANRxMsg->Data[3] >> 0)) * 0.2) - 204.6;
    CAN1_Obj_1_Msg.Obj_DynProp   = (CANRxMsg->Data[6] >> 0) & 0x07;
    CAN1_Obj_1_Msg.Obj_VerlLong  = (((CANRxMsg->Data[4] & 0xFF) << 2) | (CANRxMsg->Data[5] >> 6)) * 0.25 - 128;
    CAN1_Obj_1_Msg.Obj_VerlLat   = (((CANRxMsg->Data[5] & 0x3F) << 3) | ((CANRxMsg->Data[6] & 0xE0) >> 5)) * 0.25 - 64;

    y = (CAN1_Obj_1_Msg.Obj_LatDispl - tan_angle * CAN1_Obj_1_Msg.Obj_LongDispl) / sqrt_angle;
    x = (CAN1_Obj_1_Msg.Obj_LongDispl - tan_angle * CAN1_Obj_1_Msg.Obj_LatDispl) / sqrt_angle;

    CAN1_Obj_1_Msg.Obj_LatDispl  = (float)y;
    CAN1_Obj_1_Msg.Obj_LongDispl = (float)x;

    y = (CAN1_Obj_1_Msg.Obj_VerlLat - tan_angle * CAN1_Obj_1_Msg.Obj_LongDispl) / sqrt_angle;
    x = (CAN1_Obj_1_Msg.Obj_VerlLong - tan_angle * CAN1_Obj_1_Msg.Obj_VerlLat) / sqrt_angle;

    CAN1_Obj_1_Msg.Obj_VerlLat  = (float)y;
    CAN1_Obj_1_Msg.Obj_VerlLong = (float)x;

    objData.Obj_ID0 = 0;
    objData.Obj_ID1 = (CANRxMsg->Data[0] >> 0);

    objData.Obj_LongDispl0 = ((int16_t)(CAN1_Obj_1_Msg.Obj_LongDispl * 10)) >> 8;
    objData.Obj_LongDispl1 = ((int16_t)(CAN1_Obj_1_Msg.Obj_LongDispl * 10)) & 0xff;
    objData.Obj_LatDispl0  = ((int16_t)(CAN1_Obj_1_Msg.Obj_LatDispl * 10)) >> 8;
    objData.Obj_LatDispl1  = ((int16_t)(CAN1_Obj_1_Msg.Obj_LatDispl * 10)) & 0xff;
    objData.Obj_VerlLong0  = ((int16_t)(CAN1_Obj_1_Msg.Obj_VerlLong * 100)) >> 8;
    objData.Obj_VerlLong1  = ((int16_t)(CAN1_Obj_1_Msg.Obj_VerlLong * 100)) & 0xff;

    if (optData.D_VStatus) {
        objData.Obj_VerlLat0 = ((int16_t)(CAN1_Obj_1_Msg.Obj_VerlLat * 100)) >> 8;
        objData.Obj_VerlLat1 = ((int16_t)(CAN1_Obj_1_Msg.Obj_VerlLat * 100)) & 0xff;
    } else {
        cache                = (int16_t)(100 * CAN1_Obj_1_Msg.Obj_LongDispl / (-CAN1_Obj_1_Msg.Obj_VerlLong));
        objData.Obj_VerlLat0 = cache >> 8;
        objData.Obj_VerlLat1 = cache & 0xff;
    }

    objCouonts++;
    // printf("ID:%02d,objCouonts:%d NoOfObjects:%d\r\n",CAN1_Obj_1_Msg.Obj_ID, objCouonts,objStatus.NoOfObjects);

    if (CAN1_Obj_1_Msg.Obj_DynProp == MOVING) {
        if (movingCount < 10) {
            memcpy(&objMovingData[movingCount], &objData, sizeof(ObjStruct_t));
            movingCount++;
        }
    } else if (CAN1_Obj_1_Msg.Obj_DynProp == ONCOMING)  // ONCOMING
    {
        if (((optData.rightWidth < (CAN1_Obj_1_Msg.Obj_LatDispl * 10)) && (CAN1_Obj_1_Msg.Obj_LatDispl > 0)))
            goto objExit;
        if ((-optData.leftWidth > (CAN1_Obj_1_Msg.Obj_LatDispl * 10)) && (CAN1_Obj_1_Msg.Obj_LatDispl < 0))
            goto objExit;
        if (optData.startWarnDistance < (CAN1_Obj_1_Msg.Obj_LongDispl * 10))
            goto objExit;
        // if ((CAN1_Obj_1_Msg.Obj_VerlLong < WARNSPEED) && (CAN1_Obj_1_Msg.Obj_LongDispl > DISTANCE_LOWSPEED_WARN))
        //     goto objExit;
        if (oncomingCount < 10) {
            //	printf("time:%d,x:%f,speed:%f\r\n",(int16_t)
            //(CAN1_Obj_1_Msg.Obj_LongDispl/(-CAN1_Obj_1_Msg.Obj_VerlLong)),CAN1_Obj_1_Msg.Obj_LongDispl
            //,CAN1_Obj_1_Msg.Obj_VerlLong );
            memcpy(&objOncomingData[oncomingCount], &objData, sizeof(ObjStruct_t));
            oncomingCount++;
        }
    } else if (CAN1_Obj_1_Msg.Obj_DynProp == CROSING_MOVING) {
        if (crossmovingCount < 10) {
            memcpy(&objCrossmovingData[crossmovingCount], &objData, sizeof(ObjStruct_t));
            crossmovingCount++;
        }
    }
objExit:
    if (objCouonts >= objStatus.NoOfObjects) {
        radarDataProcessing();
    }

    return CAN1_Obj_1_Msg;
}
void radarDataProcessing(void)
{
    u8          i, j;
    ObjStruct_t objTemp;

    for (i = 0; i < oncomingCount - 1; i++) {
        for (j = 0; j < oncomingCount - 1 - i; j++) {
            u16 x0, x1;
            if (optData.D_VStatus) {
                x0 = (objOncomingData[j].Obj_LongDispl0 << 8) | objOncomingData[j].Obj_LongDispl1;
                x1 = (objOncomingData[j + 1].Obj_LongDispl0 << 8) | objOncomingData[j + 1].Obj_LongDispl1;
            } else {
                x0 = (objOncomingData[j].Obj_VerlLat0 << 8) | objOncomingData[j].Obj_VerlLat1;
                x1 = (objOncomingData[j + 1].Obj_VerlLat0 << 8) | objOncomingData[j + 1].Obj_VerlLat1;
            }
            if ((x0 > x1) || ((x0 == 0) && (x1 != 0))) {
                memcpy(&objTemp, &objOncomingData[j + 1], sizeof(ObjStruct_t));
                memcpy(&objOncomingData[j + 1], &objOncomingData[j], sizeof(ObjStruct_t));
                memcpy(&objOncomingData[j], &objTemp, sizeof(ObjStruct_t));
            }
        }
    }
    memcpy(&objOncomingBuff[0], &objOncomingData[0], oncomingCount * sizeof(ObjStruct_t));
    //  printf("x0:%d\r\n", (objOncomingBuff[0].Obj_VerlLat0 << 8) | objOncomingBuff[0].Obj_VerlLat1);
    if (oncomingCount < 10)
        memset(&objOncomingBuff[oncomingCount], 0, sizeof(ObjStruct_t) * (10 - oncomingCount));
    radarChangeFlag = 1;
}
/**
 * @brief
 *
 * @param CANRxMsg
 * @return struct ClusterList
 */
struct ClusterList ASR_Cluster_Handle(CanRxMsg *CANRxMsg)
{
#define ARS408
    struct ClusterList CAN1_Cluster_Msg;
#ifdef ARS408
    CAN1_Cluster_Msg.ID       = (CANRxMsg->Data[0] >> 0);                                                        // 0->255
    CAN1_Cluster_Msg.DistLong = ((CANRxMsg->Data[1] << 5) | (CANRxMsg->Data[2] >> 3)) * 0.2 - 500;               // x
    CAN1_Cluster_Msg.DistLat  = ((((CANRxMsg->Data[2] & 0x03) << 8) | (CANRxMsg->Data[3] >> 0)) * 0.2) - 102.3;  // y
    CAN1_Cluster_Msg.VrelLong = (((CANRxMsg->Data[4] & 0xFF) << 2) | (CANRxMsg->Data[5] >> 6)) * 0.25 - 128;     // x speed
    CAN1_Cluster_Msg.DynProp  = (CANRxMsg->Data[6] >> 0) & 0x07;
    CAN1_Cluster_Msg.VrelLat  = (((CANRxMsg->Data[5] & 0x3F) << 3) | ((CANRxMsg->Data[6] & 0xE0) >> 5)) * 0.25 - 64;  // y speed

#endif
    if ((CAN1_Cluster_Msg.DynProp == 0) || (CAN1_Cluster_Msg.DynProp == 2) || (CAN1_Cluster_Msg.DynProp == 6))
        printf("ClusterID:%02d X:%.01f\tY:%.01f\tDynProp:%d X speed:%.02f\tY speed:%.02f\r\n", CAN1_Cluster_Msg.ID, CAN1_Cluster_Msg.DistLong, CAN1_Cluster_Msg.DistLat, CAN1_Cluster_Msg.DynProp,
               CAN1_Cluster_Msg.VrelLong, CAN1_Cluster_Msg.VrelLat);
    if (CAN1_Cluster_Msg.ID < 20)
        memcpy(&clusterBuff[CAN1_Cluster_Msg.ID], &CAN1_Cluster_Msg, sizeof(ClusterList_t));
    return CAN1_Cluster_Msg;
}
